﻿using System;
using System.Collections.Generic;
using System.Diagnostics;

namespace NestedParticleFilter
{

    /// <summary>
    /// K-Means Clustering
    /// </summary>
    public class Cluster
    {
        private const double ConverganceThreshold = 0.5;
        private readonly List<RobotPose> _dataPoints;
        private readonly int _size;          // size of the k-mean cluster
        private List<RobotPose>[] _clusters; // array of list... for each cluster we have a list... this array is of size k
        private RobotPose[] _means;           // mean postion of each cluster


        #region Constructors
        /// <summary>
        /// Default constructor. The default size of the clustering is 2
        /// </summary>
        public Cluster()
        {
            _size = 2; 
            _clusters = new List<RobotPose>[_size];
            for (int i = 0; i < _size; i++)
                _clusters[i] = new List<RobotPose>();
            _means = new RobotPose[_size];
            _dataPoints = new List<RobotPose>();
           
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="size"></param>
        public Cluster(int size)
        {
            this._size = size;
            _clusters = new List<RobotPose>[size];
            for (int i = 0; i < size; i++)
                _clusters[i] = new List<RobotPose>();
            _means = new RobotPose[size];
            _dataPoints = new List<RobotPose>();
        }
        #endregion 

        #region Public Methods
        public RobotPose[] Means
        {
            get { return _means; }
        }

        /// <summary>
        /// Each particle location will be a data point for our clustering algorithm
        /// </summary>
        /// <param name="particles"></param>
        private void SetDataPoints(List<Particle> particles)
        {
            ClearDataPoints();
            for (int i = 0; i < particles.Count; i++)
            {
                _dataPoints.Add((RobotPose)particles[i].Pose.Clone());
            }
        }

        /// <summary>
        /// Initializes the means and iteratively assign datapoints to each cluster untill no change is made
        /// </summary>
        public void PerformClustering(List<Particle> particles)
        {
            SetDataPoints(particles);
            bool changed;
            InitializeMeans();
            do
            {
                AssignDatapointsToCluster();
                changed = ComputeClusterMean();
            } while (changed);
        }



        /// <summary>
        /// returns the mean of most densed cluster
        /// </summary>
        /// <returns></returns>
        public RobotPose MeanOfMostDensedCluster()
        {
            int maxSize = 0;
            int densedCluster = -1; //index of the populated cluster
            // find the most populated cluster and its index... Clusters are indexed from [0-k)
            for (int i = 0; i < _size; i++)
            {
                if (_clusters[i].Count >= maxSize)
                {
                    maxSize = _clusters[i].Count;
                    densedCluster = i;
                }
            }
            // return the mean value of the most populated cluster
            return _means[densedCluster];
        }
        #endregion

        /// <summary>
        /// Computes each cluster's mean and return true if the mean is changed
        /// </summary>
        /// <returns>true of the mean is changed and false otherwise</returns>
        private bool ComputeClusterMean()
        {
            bool changed = false;
            // for each cluster
            for (int i = 0; i < _size; i++)
            {
                double sumX = 0;
                double sumZ = 0;
                double xtheta = 0, ztheta = 0, theta = 0; ;
                // calculate the averaged x and z points
                for (int index = 0; index < _clusters[i].Count; index++)
                {
                    sumX += _clusters[i][index].X;
                    sumZ += _clusters[i][index].Z;
                    xtheta += Math.Sin(UnitConversion.DegreeToRadian(_clusters[i][index].Theta));
                    ztheta += Math.Cos(UnitConversion.DegreeToRadian(_clusters[i][index].Theta));
                   
                }
                if (_clusters[i].Count != 0)
                {
                    sumX /= _clusters[i].Count;
                    sumZ /= _clusters[i].Count;
                    theta = Math.Atan2(xtheta, ztheta);
                    theta = UnitConversion.RadianToDegree(theta);
                }
                // if the new mean is slightly changed then ignore otherwise assign the new mean.
                if (Math.Abs(_means[i].X - sumX) > ConverganceThreshold
                    && Math.Abs(_means[i].Z - sumZ) > ConverganceThreshold)
                {
                    _means[i].X = sumX;
                    _means[i].Z = sumZ;
                    _means[i].Theta = theta;
                    changed = true;
                }
            }
            return changed;
        }

        /// <summary>
        /// Clears the data points
        /// </summary>
        private void ClearDataPoints()
        {
            _dataPoints.Clear();
        }
      
        /// <summary>
        /// clears out all the clusters
        /// </summary>
        private void ClearClusters()
        {
            for (int i = 0; i < _size; i++)
                _clusters[i].Clear();
        }

        /// <summary>
        /// Assign each data point to a cluster based on how close they are to each cluser's mean point
        /// </summary>
        private void AssignDatapointsToCluster()
        {
            ClearClusters();
            for (int i = 0; i < _dataPoints.Count; i++)
            {
                int meanIndex = FindNearestCluster(_dataPoints[i]);
                _clusters[meanIndex].Add(_dataPoints[i]);
            }
        }

        /// <summary>
        /// Find the nearest cluster to the pose p. This function is used in AssignDataPointsToCluster
        /// </summary>
        /// <param name="p">Pose</param>
        /// <returns> the index of that cluster </returns>
        private int FindNearestCluster(RobotPose p)
        {
            double min = 1000000;
            int minIndex = -1;
            //for all the means... find the closest mean.
            for (int i = 0; i < _means.Length; i++)
            {
                double distance =
                    Math.Sqrt(Math.Pow(p.X - _means[i].X, 2) + Math.Pow(p.Z - _means[i].Z, 2));
                if (distance < min)
                {
                    min = distance;
                    minIndex = i;
                }
            } //end of for
            Debug.Assert(minIndex != -1, " Min index cannot be negative");
            return minIndex;
        }

        
        /// <summary>
        /// Picks random points in the map and initialize them as means
        /// </summary>
        private void InitializeMeans()
        {
            
            for (int i = 0; i < _size; i++)
            {
                var rand = new Random(DateTime.Now.Millisecond*i);
                int index = rand.Next(_dataPoints.Count);
                if (_dataPoints.Count != 0)
                    _means[i] = (RobotPose) (_dataPoints[index].Clone());
            }
        }
    }
}